/*
    This file is part of Slydini.

    Slydini is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Slydini is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Slydini.  If not, see <http://www.gnu.org/licenses/>.
*/
/* 
 * File:   frameevaluation.cpp
 * Author: kajetan
 * 
 * Created on 28 marzec 2009, 13:55
 */

#include "frameevaluation.h"

FrameEvaluation *FrameEvaluation::instance = NULL;

FrameEvaluation *FrameEvaluation::getInstance() {
	if (!instance)
		instance = new FrameEvaluation();
	return instance;
}

FrameEvaluation::FrameEvaluation() {
	prev_frame = NULL;
	mode = 0;
	unrecognizedFrames = 0;

	hand.clear();
	hand_prev.clear();

	Configuration* c = Configuration::getConfiguration("configuration.xml");
	varColor = c->queryInt("frame_evaluation/varColor");
	fingerColor = CV_RGB(c->queryInt("finger_color/redColor"),c->queryInt("finger_color/greenColor"),c->queryInt("finger_color/blueColor"));

}

void FrameEvaluation::updateConfiguration() {
	Configuration* c = Configuration::getConfiguration("configuration.xml");
	varColor = c->queryInt("frame_evaluation/varColor");
	fingerColor = CV_RGB(c->queryInt("finger_color/redColor"),c->queryInt("finger_color/greenColor"),c->queryInt("finger_color/blueColor"));
}


Hand FrameEvaluation::getHand() {
	return hand;
}

void FrameEvaluation::findMyEllipses(IplImage* img, vector<MyEllipse>& ret) {
	CvMemStorage* stor;
	CvSeq* cont;
	CvBox2D32f* box;
	CvPoint* PointArray;
	CvPoint2D32f* PointArray2D32f;

	stor = cvCreateMemStorage(0);
	cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);

	IplImage* imgThrs = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);

	int slider_pos = 70;
	cvThreshold(img, imgThrs, slider_pos, 255, CV_THRESH_BINARY);

	cvFindContours(imgThrs, stor, &cont, sizeof (CvContour),
			CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0, 0));

	cvZero(imgThrs);

	for (; cont; cont = cont->h_next) {
		int i; // Indicator of cycle.
		int count = cont->total; // This is number point in contour
		CvPoint center;
		CvSize size;

		// Number point must be more than or equal to 6 (for cvFitMyEllipse_32f).
		if (count < 6)
			continue;

		// Alloc memory for contour point set.
		PointArray = (CvPoint*) malloc(count * sizeof (CvPoint));
		PointArray2D32f = (CvPoint2D32f*) malloc(count * sizeof (CvPoint2D32f));

		// Alloc memory for MyEllipse data.
		box = (CvBox2D32f*) malloc(sizeof (CvBox2D32f));

		// Get contour point set.
		cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ);

		// Convert CvPoint set to CvBox2D32f set.
		for (i = 0; i < count; i++) {
			PointArray2D32f[i].x = (float) PointArray[i].x;
			PointArray2D32f[i].y = (float) PointArray[i].y;
		}

		// Fits MyEllipse to current contour.
		cvFitEllipse(PointArray2D32f, count, box);

		// Draw current contour.
		// cvDrawContours(image04, cont, CV_RGB(255, 255, 255), CV_RGB(255, 255, 255), 0, 1, 8, cvPoint(0, 0));

		// Convert MyEllipse data from float to integer representation.
		center.x = cvRound(box->center.x);
		center.y = cvRound(box->center.y);
		size.width = cvRound(box->size.width * 0.5);
		size.height = cvRound(box->size.height * 0.5);
		box->angle = -box->angle;

		if (size.width * size.height > 30 && size.width * size.height < 400) {
			// cvMyEllipse(image04, center, size, box->angle, 0, 360, CV_RGB(0, 0, 255), 1, CV_AA, 0);
			//            std::cout << center.x << " " << center.y << std::endl;
			MyEllipse tmp;
			tmp.center.x = center.x; 
			tmp.center.y = center.y; 
			tmp.size = size; tmp.angle = box->angle;
			//            std::cout << center.x << " " << center.y << std::endl;
			ret.push_back(tmp);
			//            std::cout << tmp.center.x << " " << tmp.center.y << std::endl;
			//            std::cout << ret[0].center.x << " " << ret[0].center.y << std::endl;
		}

		// Free memory.
		free(PointArray);
		free(PointArray2D32f);
		free(box);
	}

	cvReleaseImage(&imgThrs);
	cvReleaseMemStorage( &stor);

}

void FrameEvaluation::selectPointersArea(IplImage* picture, IplImage* pointers, CvScalar color, bool flag) {
	RgbImage* pictureIm = new RgbImage(picture);
	BwImage* pointersIm = new BwImage(pointers);

	int h = pointersIm->imgp->height;
	int w = pointersIm->imgp->width;
	int z;

	for (int i = 0; i < h; i++)
		for (int j = 0; j < w; j++) {
			/*            if (((int) color.val[2]-(*pictureIm)[i][j].r)*((int) color.val[2]-(*pictureIm)[i][j].r)+
				      ((int) color.val[1]-(*pictureIm)[i][j].g)*((int) color.val[1]-(*pictureIm)[i][j].g)+
				      ((int) color.val[0]-(*pictureIm)[i][j].b)*((int) color.val[0]-(*pictureIm)[i][j].b) < VARCOLOR) {
			//                (*pointersIm)[i][j].b = 0, (*pointersIm)[i][j].g = 0, (*pointersIm)[i][j].r = 0;
			(*pointersIm)[h-i-1][j] = 0;
			} else {
			//                (*pointersIm)[i][j].b = 255, (*pointersIm)[i][j].g = 255, (*pointersIm)[i][j].r = 255;
			(*pointersIm)[h-i-1][j] = 255;
			}*/
			if((4*i<h || 4*i>h*3 || 4*j<w || 4*j>w*3) && !flag)
				z = 255;
			else 
				z = (((int)color.val[2]-(*pictureIm)[i][j].r)*((int) color.val[2]-(*pictureIm)[i][j].r)+
						((int)color.val[1]-(*pictureIm)[i][j].g)*((int) color.val[1]-(*pictureIm)[i][j].g)+
						((int)color.val[0]-(*pictureIm)[i][j].b)*((int) color.val[0]-(*pictureIm)[i][j].b))/varColor*255;
			(*pointersIm)[i][j] = (z<255 ? 255-z : 0);
			//			(*pointersIm)[h-i-1][j] *= ((*pointersIm)[h-i-1][j]>=0 ? 1 : 0);
		}
	//    IplImage* tmp = cvCreateImage(cvSize(picture->width, picture->height), IPL_DEPTH_8U, 3);
	//    cvCopy(pointersIm->imgp, tmp);

	/*
	 * cvPyrDown(picture, tmp);
	 * cvDilate(picture, picture, 0, 2);
	 * cvPyrUp(tmp, picture);
	 * cvErode(picture, picture, 0, 1);
	 */
	//    cvSmooth(tmp, picture, CV_BLUR, 3, 3);
	//    cvReleaseImage(&tmp);
	//    IplImage* grim = cvCreateImage(cvSize(pointers->width, pointers->height), IPL_DEPTH_8U, 1);

	//    cvCvtColor(pointers, grim, CV_BGR2GRAY);
	cvCopy(pointersIm->imgp, pointers);

	//    cvSaveImage("result.jpg", pointers);

	delete pictureIm;
	delete pointersIm;
}


void FrameEvaluation::chooseEllipsesForFingers(vector<MyEllipse>& ellipses) 
{ 	 
	int n=ellipses.size();
	for(int i=0; i<n; ++i)
		for(int j=i+1; j<n; ++j)
			if( abs(ellipses[i].center.y-ellipses[j].center.y) < 
				2*(ellipses[i].size.height+ellipses[j].size.height) &&
				abs(ellipses[i].center.x-ellipses[j].center.x) < 
				5*(ellipses[i].size.width+ellipses[j].size.width) && 
				abs(ellipses[i].center.x-ellipses[j].center.x) > 
				2*(ellipses[i].size.width+ellipses[j].size.width))	{
				if(ellipses[i].center.x<ellipses[j].center.x) {
					hand[0].TIP = ellipses[i];
					hand[1].TIP = ellipses[j];
				} else {
					hand[0].TIP = ellipses[j];
					hand[1].TIP = ellipses[i];
				}
				hand[0].ACTIVE = true;
				hand[1].ACTIVE = true;
			}
}


void FrameEvaluation::callLucasKanadeOnFingers(IplImage* frame) 
{
	IplImage* grey = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );

	cvCvtColor( frame, grey, CV_BGR2GRAY );
	cvCvtColor( prev_frame, prev_grey, CV_BGR2GRAY );

	CvPoint2D32f* points[2] = {0,0};
	points[0] = (CvPoint2D32f*)cvAlloc(5*sizeof(points[0][0]));
	points[1] = (CvPoint2D32f*)cvAlloc(5*sizeof(points[0][0]));

	char* status = (char*)cvAlloc(5);

	int count=2;
	//	 printf("(%d, %d)\n",prev_fingers->tips[0].center.x,prev_fingers->tips[0].center.x);
	for(int i=0; i<count; ++i) {
		points[0][i] = cvPointTo32f(hand_prev[i].TIP.center);
		cvCircle(grey, hand_prev[i].TIP.center, 30, CV_RGB(255,255,255), 1);
	}
	cvShowImage( "BW", grey );

	int winsize = 20, flags = 0;
	cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
			points[0], points[1], count, cvSize(winsize,winsize), 3, status, 0,
			cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );

	int k=-1, l;
	for(int i=0; i<count; ++i) {
		if( !status[i] || !hand_prev[i].ACTIVE)
			k = i;
		else {
 			hand[i].TIP.center = cvPointFrom32f(points[1][i]);
			hand[i].ACTIVE = true;
		}
	}
//	if(!k) printf("BŁĄD\n");
	l = (k==0 ? 1 : (k==1 ? 0 : -1));

	if(k!=-1 && hand[l].ACTIVE) {
		hand[k].TIP = hand_prev[k].TIP;
		hand[k].TIP.center.x += hand[l].TIP.center.x - hand_prev[l].TIP.center.x;
		hand[k].TIP.center.y += hand[l].TIP.center.y - hand_prev[l].TIP.center.y;
	}
	else if(hand.empty()) {
		printf("BŁĄD\n");
	}

	cvFree(&points[0]);
	cvFree(&points[1]);
	cvFree(&status);

	cvReleaseImage(&grey);
	cvReleaseImage(&prev_grey);
	cvReleaseImage(&pyramid);
	cvReleaseImage(&prev_pyramid);

}

//void FrameEvaluation::verifyFingers(IplImage* frame, vector<bool>& ver)
void FrameEvaluation::verifyFingers(IplImage* frame)
{
	IplImage* bw_frame = cvCreateImage( cvGetSize(frame), 8, 1 );
	FrameEvaluation::selectPointersArea(frame, bw_frame, CV_RGB(255,0,0), 1);
	vector<MyEllipse> ellipses;

	FrameEvaluation::findMyEllipses(bw_frame, ellipses);

	int i=(hand[0].ACTIVE ? 0 : 1), s = -1;
	
	hand[0].ACTIVE = false;
	hand[1].ACTIVE = false;

	int mnDst = 10000000;
	for(unsigned int j=0; j<ellipses.size(); ++j)
		if(hand[i].TIP==ellipses[j] && MyEllipse::dstSq(hand[i].TIP, ellipses[j])<mnDst) {
			hand[i].TIP = ellipses[j];
			hand[i].ACTIVE = true;
			s=j; 
			mnDst = MyEllipse::dstSq(hand[i].TIP, ellipses[j]);
			break;
		}

	int k=(i==0 ? 1 : 0);
	for(unsigned int j=0; j<ellipses.size(); ++j)
		if(j!=(unsigned int)s && hand[k].TIP==ellipses[j]) {
			hand[k].TIP = ellipses[j];
			hand[k].ACTIVE = true;
			break;
		}	

	if(!hand[k].ACTIVE) {
		hand[k].TIP = hand_prev[k].TIP;
		hand[k].TIP.center.x += hand[i].TIP.center.x - hand_prev[i].TIP.center.x;
		hand[k].TIP.center.y += hand[i].TIP.center.y - hand_prev[i].TIP.center.y;
	}

	cvReleaseImage(&bw_frame);

}

void FrameEvaluation::update(IplImage* frame)
{
	if (!prev_frame) {
		prev_frame = cvCreateImage(cvSize(frame->width, frame->height), IPL_DEPTH_8U, frame->nChannels);
		printf("cos\n");
	}

//	printf("fingers on stack: %d\n",(int)prev_fingers.size());

	// if there's no initial estimation for fingers positions
	if(hand_prev.empty()) {
		IplImage* bw_frame = cvCreateImage( cvGetSize(frame), 8, 1 );
		FrameEvaluation::selectPointersArea(frame, bw_frame, fingerColor, 0);
		//        cvCvtColor( frame, bw_frame, CV_RGB2GRAY );
		cvShowImage( "BW", bw_frame );
		vector<MyEllipse> ellipses;
		FrameEvaluation::findMyEllipses(bw_frame, ellipses);
		printf("ellipses found: %d\n", (int)ellipses.size());

		//	 	printf("(%d, %d)\n",ellipses[0].center.x,ellipses[0].center.y);
		if(ellipses.size()>0) {
			hand.clear();							  
			FrameEvaluation::chooseEllipsesForFingers(ellipses);
			if(hand.empty()) {
				printf("hand: empty\n");
			}
		}
		//		printf("%s\n",(fingers==NULL?"błąd":"ok"));
		cvReleaseImage(&bw_frame);		 				 
	}
	// otherwise - track previous ones
	else {
		if (!mode) {
			hand.clear();
			FrameEvaluation::callLucasKanadeOnFingers(frame);
//			printf("fingers tracked: %d\n", (int)fingers.size());

//			vector<bool> ver(2,0);
//			FrameEvaluation::verifyFingers(frame, ver);
			FrameEvaluation::verifyFingers(frame);

//			hand[0].ACTIVE = ver[0];
//			hand[1].ACTIVE = ver[1];

		} else {

		}
	} 

	if(!(hand.empty())) {
//         printf("fingers found: %d\n", (int)fingers.size());
         hand_prev = hand;
         unrecognizedFrames = 0;
    }
    else {
    	 ++unrecognizedFrames;
         if(unrecognizedFrames > 10) {
         	unrecognizedFrames = 0;
        	hand_prev.clear();
         }
    	 printf("fingers found - none: %d\n", 0);                         
    }
    printf("%d\n",(int)(hand[0].ACTIVE)+(int)(hand[1].ACTIVE));       

    cvShowImage( "CamShiftDemo", frame );
//    fingers.clear(); 
//	hand.clear();

	cvCopy(frame,prev_frame,0);
}
